/*-----------------------------------------------------------------------------
 * @file   bat_zhiann.c
 * @brief  Driver for a zhiann Smart Battery by CAN BUS
 * @author WLH<wanglehui8357@163.com> Copyright (C) 2021~2099.
----------------------------------------------------------------------------*/
#include "bat_zhiann.h"
#include "dbg_tool.h"
#include "hal_delay.h"
#include "hal_hrt.h"

/*-----------------------------------------------------------------------------
 * Macro define
-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
 * Battery Status Flags
-----------------------------------------------------------------------------*/
#define BAT_STATUS_POWER_ON   0x01 // Currently used as a power supply
#define BAT_STATUS_CHARGE_ON  0x02 // Charge mos open
#define BAT_STATUS_CHARGE_OFF 0x04 // Charge mos off
#define BAT_STATUS_CHARGING   0x08 // Is Charging
#define BAT_STATUS_CHARGED    0x10 // Charging complete

/*-----------------------------------------------------------------------------
 * local variable
-----------------------------------------------------------------------------*/
// From 1092.BatteryInfo.uavcan
typedef uint16_t FP16; /* 16-bit floating point */

/*-----------------------------------------------------------------------------
 * local functions
-----------------------------------------------------------------------------*/
#define MASK_STA_FLAG_IN_USE       BIT8(0) // The battery is currently used as a power supply
#define MASK_STA_FLAG_CHARGING     BIT8(1) // Charger is active
#define MASK_STA_FLAG_CHARGED      BIT8(2) // Charging complete, but the charger is still active
#define MASK_STA_FLAG_TEMP_HOT     BIT8(3) // Battery temperature is above normal
#define MASK_STA_FLAG_TEMP_COLD    BIT8(4) // Battery temperature is below normal
#define MASK_STA_FLAG_OVERLOAD     BIT8(5) // Safe operating area violation
#define MASK_STA_FLAG_BAD_BATTERY  BIT8(6) // This battery should not be used anymore (e.g. low SOH)
#define MASK_STA_FLAG_NEED_SERVICE BIT8(7) // This battery requires maintenance (e.g. balancing, full recharge)
#define MASK_STA_FLAG_BMS_ERROR    BIT8(8) // Battery management system/controller error, smart battery interface error

static void print_status_flags(const U16 status_flags)
{
    if(status_flags & MASK_STA_FLAG_IN_USE)       { trace_info("[ZHAN]:Bat is in use");        }
    if(status_flags & MASK_STA_FLAG_CHARGING)     { trace_info("[ZHAN]:Charger is active");    }
    if(status_flags & MASK_STA_FLAG_CHARGED)      { trace_info("[ZHAN]:Charging complete");    }
    if(status_flags & MASK_STA_FLAG_TEMP_HOT)     { trace_warn("[ZHAN]:Temp is above normal"); }
    if(status_flags & MASK_STA_FLAG_TEMP_COLD)    { trace_warn("[ZHAN]:Temp is below normal"); }
    if(status_flags & MASK_STA_FLAG_OVERLOAD)     { trace_warn("[ZHAN]:Bat overload");         }
    if(status_flags & MASK_STA_FLAG_BAD_BATTERY)  { trace_error("[ZHAN]:Should not be used");  }
    if(status_flags & MASK_STA_FLAG_NEED_SERVICE) { trace_error("[ZHAN]:Requires maintenance");}
    if(status_flags & MASK_STA_FLAG_BMS_ERROR)    { trace_error("[ZHAN]:BMS error");           }
}


static inline FP32 fp16_to_fp32(FP16 value)
{
    union U_FP32
    {
        U32 u;
        FP32 f;
    };
    const union U_FP32 magic = { (254UL - 15UL) << 23U };
    const union U_FP32 was_inf_nan = { (127UL + 16UL) << 23U };
    union U_FP32 out;
    out.u = (value & 0x7FFFU) << 13U;
    out.f *= magic.f;
    if (out.f >= was_inf_nan.f)
    {
        out.u |= 255UL << 23U;
    }
    out.u |= (value & 0x8000UL) << 16U;
    return out.f;
}
/**
 * @brief decode BatteryInfo, return true on failure, false on success
*/
static bool battery_info_decode(zhiann_bat_t* bat, const U8* data, const U8 len)
{
    int ofs = 0;
    bat->tmp = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->volt = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->curr = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->avg_power_10sec = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->remaining_cap_wh = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->full_charge_cap_wh = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;
    bat->hours_to_full_charge = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2;

    const U16 status_flags = GEN_WORD(data[ofs], data[ofs+1]>>5);// 11 Bits(8+3)
    if(bat->status_flags != status_flags)
    {
        bat->status_flags = status_flags;
        print_status_flags(status_flags);
    }
    ofs += 1;

    bat->soh = data[ofs] & 0x1F; // 5Bit
    bat->soh = bat->soh << 2 | (data[ofs+1]>>6);// 7 Bits(5+2)
    ofs += 1;

    bat->soc = data[ofs] & 0x3f; //6Bit
    bat->soc = bat->soc <<1 | (data[ofs+1]>>7);//7Bit(6+1)
    ofs += 1;

    bat->soc_stdev = data[ofs] & 0x7f;// 7 Bits
    ofs += 1;

    bat->dev_id = data[ofs]; // battery_id
    ofs += 1;

    // const U32 model_instance_id =  BUF_TO_U32(data, ofs);
    ofs += 4;
    //model_name; // 5Bits

    bat->pct = bat->soc;
    bat->ts = hal_hrt_get_ms();
    return true;
}
/**
 * @brief decode BatteryInfo, return true on failure, false on success
*/
static bool battery_aux_info_decode(zhiann_bat_t* bat, const U8* data, const U8 len)
{
    int ofs = 0;
    ofs += 7; // UAVCAN timestamp ignore
    bat->cell_cnt = data[ofs++];

    // Battery individual cell voltages
    // length of following field also used as cell count
    // float16[<=255] voltage_cell
    for(unsigned i=0u; i < bat->cell_cnt && i < ZHIANN_BAT_MAX_CELL_CNT; i++)
    {
        bat->cell_volt[i] = (U16)(1000.0f * fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1])));
        ofs += 2u;
    }

    // Number of Charge/Discharge Cycles
    // then the charge cycle count would be incremented once, not twice.
    bat->cycle_count = GEN_WORD(data[ofs], data[ofs+1]);
    ofs += 2u;

    // Number of times the battery was discharged over the rated capacity
    bat->over_discharge_count = GEN_WORD(data[ofs], data[ofs+1]);
    ofs += 2u;

    // Max instantaneous current draw since last message
    bat->max_current = fp16_to_fp32(GEN_WORD(data[ofs], data[ofs+1]));
    ofs += 2u;

    // Nominal voltage of the battery pack
    // the nominal Voltage can be used for conversion between Wh and Ah
    //  if the value of this field is 0, the conversion should be omitted.
    // float16 nominal_voltage;
    ofs += 2u;

    // Power off event imminent indication, false if unknown
    // bool is_powering_off;
    bat->is_powering_off = 0u != data[ofs];
    //ofs += 1u;

    // Identifies the battery within this vehicle, e.g. 0 - primary battery
    // uint8 battery_id

    return true;
}

/**
 * @brief decode NodeStatus, return true on failure, false on success
*/
static bool node_status_decode(zhiann_bat_t* bat, const U8* data, const U8 len)
{
    //int ofs = 0;
    //const U32 uptime_sec = BUF_TO_U32(data, ofs);
    //trace_info("[ZHAN]:uptime_sec %d", uptime_sec);

//    tpcan_decode_scalar(transfer, ofs, 32, false, &msg->uptime_sec);
//    ofs += 32;
//
//    tpcan_decode_scalar(transfer, ofs, 2, false, &msg->health);
//    ofs += 2;
//
//    tpcan_decode_scalar(transfer, ofs, 3, false, &msg->mode);
//    ofs += 3;
//
//    tpcan_decode_scalar(transfer, ofs, 3, false, &msg->sub_mode);
//    ofs += 3;
//
//    tpcan_decode_scalar(transfer, ofs, 16, false, &msg->vendor_specific_status_code);
//    ofs += 2;

    return false; /* success */
}

/**
 * @brief decode ZhianBatteryID, return true on failure, false on success
*/
static bool battery_id_decode(zhiann_bat_t* bat, const U8* data, const U8 len)
{
//    tpcan_decode_scalar(transfer, ofs, 8, false, &msg->main_ver);
//    ofs += 8;
//
//    tpcan_decode_scalar(transfer, ofs, 8, false, &msg->minor_ver);
//    ofs += 8;
//
//    tpcan_decode_scalar(transfer, ofs, 16, false, &msg->build_number);
//    ofs += 2;
//
//    tpcan_decode_scalar(transfer, ofs, 5, false, &msg->two_dimensional_code.len);
//    ofs += 5;

    return true; /* success */
}
/*-----------------------------------------------------------------------------
 * public functions
-----------------------------------------------------------------------------*/
bool zhiann_bat_init(zhiann_bat_t* bat, const U8 bus_id)
{
    const bool ret = hal_can_init(bus_id);
    bat->bus_id = ret ? bus_id : HAL_CAN_NULL;
    return ret;
}

void zhiann_bat_analyze(zhiann_bat_t* bat, const U16 msg_id, const U8* data, const U8 len)
{
    if (ZHIANN_NODE_STATUS_ID == msg_id)
    {
        node_status_decode(bat, data, len);
    }

    if (ZHIANN_BAT_ID1_ID == msg_id ||
        ZHIANN_BAT_ID2_ID == msg_id)
    {
        battery_id_decode(bat, data, len);
    }

    if (ZHIANN_BAT_CMD_ID == msg_id)
    {
    }

    if (ZHIANN_BAT_INFO_ID == msg_id)
    {
        battery_info_decode(bat, data, len);
    }

    if (ZHIANN_BAT_AUX_INFO_ID == msg_id)
    {
        battery_aux_info_decode(bat, data, len);
    }
}